/*---------------------------------------------------------------------
 * File name: uorb.cpp
 *
 * Copyright (c) <2020-2022>, <ChenLong>
 *
 * All rights reserved.
 *
 * Author: ChenLong
 * Email: worldlong@foxmail.com
 *--------------------------------------------------------------------*/
/*
*/
#include <string.h>
#include "uorb.h"
/*
*/
static UORBNode *id_list = 0;
/*
*/
UORB::UORB()
{
  _node.type = UORB_TYPE_NONE;
  _node.id = -1;
  _node.size = 0;
  _node.data = 0;
  _node.thread = 0;
  _node.id_next = 0;
  _node.next = &_node;
  _node.prev = &_node;
}
UORB::UORB(void *data)
{
  _node.type = UORB_TYPE_NONE;
  _node.id = -1;
  _node.size = 0;
  _node.data = data;
  _node.id_next = 0;
  _node.next = &_node;
  _node.prev = &_node;
}
/*
*/
void UORB::init(int id, UORBType type)
{
  _node.type = type;
  _node.id = id;
  UORBNode *id_node = id_list;
  while(id_node) {
    if(id_node->id == id) { //ID already esixted, insert into the list
      _node.next = id_node;
      _node.prev = id_node->prev;
      id_node->prev->next = &_node;
      id_node->prev = &_node;
      return; 
    }
    id_node = id_node->id_next;
  }
  //ID had not esixted, insert into the id_list
  _node.id_next = id_list;
  id_list = &_node;
}
/*
*/
int UORB::check(void* data)
{
  int ret = _node.size;
  if(ret > 0) {
    rt_enter_critical();
    memcpy(data, _node.data, ret);
    rt_exit_critical();
    _node.size = 0;
  }
  return ret;
}
/*
*/
int UORB::publish(int id, void* data, int size)
{
  if(_node.type == UORB_TYPE_NONE) {
    init(id, UORB_TYPE_PUBLISHER);
  }
  
  //update data
  rt_enter_critical();
  
  _node.size = size;
  memcpy(_node.data, data, size);
  UORBNode *sub_node = _node.next;  //not publish to itself
  while(sub_node != &_node) {
    if(sub_node->type == UORB_TYPE_SUBSCRIBER) {
      sub_node->data = _node.data;
      sub_node->size = _node.size;
      if(sub_node->thread) {
        rt_thread_resume(sub_node->thread);
      }
    }
    sub_node = sub_node->next;
  }
  
  rt_exit_critical();
  return size;
}
/*
*/
int UORB::subscribe(int id, void* data, int timeout)
{
  if(_node.type == UORB_TYPE_NONE) {
    init(id, UORB_TYPE_SUBSCRIBER);
  }
  
  int ret;
  if(timeout != 0) {
    //had received data, just copy data and return
    ret = check(data);
    if(ret > 0) {
      return ret;
    }
    
    //no new data, suspend self(timeout < 0) or delay some time(timeout > 0)
    _node.thread = rt_thread_self();
    if(timeout < 0) {
      rt_thread_suspend(_node.thread);
      rt_schedule();
    } else if(timeout > 0) {
      rt_thread_mdelay(timeout);
    } 
    // if the thread will not subscribe, the thread will not be resumed again.
    _node.thread = 0;  //reset to 0
  }
  return check(data);
}




